%% 三种最小二乘法估计
% 本题例子特殊errorX1 = errorX2 = errorX3
% meanSquareErrorX1 = meanSquareErrorX3 > meanSquareErrorX2
% 前两种估计的矩阵都特别大
clear all;
clc;

%% 参数初始化
% 模型Z = HX + V
% 进行k次测量
% Z为km*1维，H为km*n维，X为n*1维
% 方差R为km*km维
k = 100;
m = 3;
n = 3;
realLocation = [100; 200; 300];
Zk = zeros(k*m, 1);
Hk = zeros(k*m, n);
Rk = eye(k*m);

%% 生成噪声标准差[2,2,4]的样本Zk和Hk
% 假设噪声是正态分布，期望为0
% 由生成V再计算Z，简化为直接生成Z
mu = realLocation;
sigma = diag([4,4,16]);
rng(1);
tmpZk = mvnrnd(mu, sigma, k);
tmpHk = eye(3);

for i = 1:1:100
    Zk(3*i-2:3*i, 1) = tmpZk(i, :);
    Hk(3*i-2:3*i, :) = tmpHk;
    Rk(3*i-2:3*i, 3*i-2:3*i) = sigma;
end

%% 古典最小二乘估计
tic
HTH = Hk'*Hk;
invHTH = inv(HTH);
estimateX1 = invHTH*Hk'*Zk;
errorX1 = estimateX1 - realLocation
meanSquareErrorX1 = invHTH*Hk'*Rk*Hk*invHTH
toc

%% 加权最小二乘估计
tic
W = inv(Rk);
estimateX2 = inv(Hk' *W *Hk) * Hk' * W * Zk;
errorX2 = estimateX2 - realLocation
meanSquareErrorX2 = inv(Hk' *Rk' *Hk)
toc

%% 递推最小二乘估计
tic
estimateXpre = Zk(1:3, 1);
meanSquareErrorPre = sigma;
Hi = tmpHk;
HiT = tmpHk';
Ri = sigma;
for i = 2:1:100
    tmp = Hi * meanSquareErrorPre * HiT + Ri;
    gainK = meanSquareErrorPre * HiT * inv(tmp);
    Zi = Zk(3*i-2:3*i, 1);
    estimateXk = estimateXpre + gainK*(Zi - Hi*estimateXpre);
    meanSquareErrorK = meanSquareErrorPre - gainK*Hi*meanSquareErrorPre;
    estimateXpre = estimateXk;
    meanSquareErrorPre = meanSquareErrorK;
end
estimateX3 = estimateXk;
errorX3 = estimateX3 - realLocation
meanSquareErrorX3 = meanSquareErrorK
toc
